/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package GUI;

import MouseSensorDriver.Calibrator;
import MouseSensorDriver.GraphListener;
import adc_lcard.ADCE140Driver;
import datasource.AirshipKitchenSource;
import datasource.DataSource;
import datasource.SlideFilter;
import java.awt.event.ActionListener;
import java.io.Serializable;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.Random;
import java.util.logging.Level;
import java.util.logging.Logger;
import javax.swing.Timer;
import midi.Note;
import midi.Tools;

/**
 *
 * @author Asko
 */
public class EndoscopExp implements Serializable {

    final int maxSize = 4000;
    int size = 0;
    long startTime = 0;
    transient boolean scanning = true;
    transient boolean stopTimerThread = false;
    ArrayList<GraphListener> scanList = new ArrayList<>();
    /**
     * Arrays which store data to graph
     *
     */
    double[] posEng = new double[maxSize];
    double[] torEng = new double[maxSize];
    double[] posAdc = new double[maxSize];
    double[] torAdc = new double[maxSize];
    double[] time = new double[maxSize];
    double measureTime  = -1  ;
    EndoscopeChannel eChannel;
    double stPos, endPos;     //start position, end position
    double stPos_adc, stPos_eng;  //adc start position, engine start position
    double endPos_adc, endPos_eng;  //adc end position, engine end position
    int srcStPos_adc, srcStPos_eng, srcEndPos_adc, srcEndPos_eng;
    /**
     * SrcStPos_adc - source start pos adc - "Raw Data". SrcStPos_eng - source
     * start pos engine - getCurPos. SrcEndPos_adc - source end pos adcd - "Raw
     * Data". SrcEndPos_eng - source end pos eng - getCurPos.
     */
    double stTor_eng, endTor_eng;
    int srcStTor_eng, srcEndTor_eng;
    double stTor_adc, endTor_adc;
    int srcStTor_adc, srcEndTor_adc;
    double coefOffsetCalibPosAdc;
    double coefPropCalibPosAdc;
    double coefOffsetCalibTorqueAdc;
    double coefPropCalibTorqueAdc;
    double homePos = 0;
    boolean timeOut = false;
    boolean airship = true;
    AirshipKitchenSource dataSource = new AirshipKitchenSource(5);
    SlideFilter filter = new SlideFilter(100);
    //Scanner thread - works until size less than maxSize
    transient Thread scan = new Thread(new Runnable() {
        @Override
        public void run() {
            Random rand = new Random();
            double posbuf, torbuf;
            clearPlots();
            while (scanning) {
                if (size < maxSize) {
                    posbuf = eChannel.getGear().getPosition();
                    torbuf = 0;
                    torbuf = eChannel.getGear().getTorque();
                    torbuf = filter.filter(torbuf);
                    if (posbuf != Double.MAX_VALUE
                            && torbuf != Double.MAX_VALUE && torbuf != 0) {
                        posEng[size] = posbuf;
                        
                        torEng[size] = torbuf + 1.3; 
                        
                        time[size] = (System.currentTimeMillis() - startTime) / 1000.0;
                        if (airship) {
                            posAdc[size] = posEng[size] + 0.2 * (rand.
                                    nextDouble() - 0.5) + time[size] * 0.2;
                           // torAdc[size] = dataSource.getData(torbuf, time[size]);
                            torAdc[size] = torEng[size] + 0.2 * (rand.nextDouble() - 0.5) + time[size] * 0.02;
                        } else {
                            posAdc[size] = eChannel.getAdcData();
                            torAdc[size] = eChannel.getTorqueData();
                        }
                        translateToPlots(posEng[size], time[size]);
                        //time[size] = size*10;
                        size++;
                    } else {
                        //System.out.println("DOUBLE MAX");
                    }
                }
//                try {
//                    Thread.sleep(50);
//                } catch (InterruptedException ex) {
//                    Logger.getLogger(EndoscopExp.class.getName()).log(Level.SEVERE, null, ex);
//                }
            }
            scanning = true;
        }
    });
    //Timer thread
    transient Thread timer = new Thread(new Runnable() {
        @Override
        public void run() {
            try {
                stopTimerThread = false;
                for (int i = 0; i < 400 && !stopTimerThread; i++) {
                    Thread.sleep(10);
                }
                if (!stopTimerThread) {
                    timeOut = true;
                }
            } catch (InterruptedException ex) {
                Logger.getLogger(EndoscopExp.class.getName()).log(Level.SEVERE, null, ex);
            }
        }
    });

    public double getCoefOffsetCalibPosAdc() {
        return coefOffsetCalibPosAdc;
    }

    public double getCoefPropCalibPosAdc() {
        return coefPropCalibPosAdc;
    }

    /**
     * Constructor
     *
     * @param eChannel
     * @param startPos
     * @param endPos
     */
    public EndoscopExp(EndoscopeChannel eChannel, double startPos, double endPos) {
        this.eChannel = eChannel;
        if (startPos >= eChannel.getGear().getMinPos() && startPos
                <= eChannel.getGear().getMaxPos()) {
            this.stPos = startPos;
        } else {
            this.stPos = 0;
            System.out.println("Stpos is 0! Soaked in shit!");
        }
        if (endPos >= eChannel.getGear().getMinPos() && endPos
                <= eChannel.getGear().getMaxPos()) {
            this.endPos = endPos;
        } else {
            this.endPos = 0;
            System.out.println("Endpos is 0! Soaked in shit!");
        }
        scan.setPriority(7);
        timer.setPriority(7);
    }

    public EndoscopExp(EndoscopeChannel eChannel, double startPos,
            double endPos, double homePos) {
        this(eChannel, startPos, endPos);
        this.homePos = homePos;

    }

    public void startExp() {
        setupEngine();
        //  System.out.println("1.Setup engine done");
        goToPosition(stPos);
        ///  System.out.println("2.Go to start position done");
        startMeasure();
        //  System.out.println("3.Starting measuring");
        goToPosition(endPos);
        measureTime = System.currentTimeMillis();
        detectTorque();
        measureTime = System.currentTimeMillis() - measureTime ;
        // System.out.println("4.Go to end position");
        endMeasure();
        //  System.out.println("5.Ending measuring");
    }
    
    public void goHome() {
        eChannel.getGear().setPosition(homePos);
        while (eChannel.getGear().getActuator().isMoving()) {
            try {
                Thread.sleep(5);
            } catch (InterruptedException ex) {
                Logger.getLogger(EndoscopExp.class.getName()).log(Level.SEVERE, null, ex);
            }
        }
    }

    /**
     * First step - setupEngine Second step - sendPosition to engine Third step
     * - start measure. Start up scanning Go to position End measuring
     */
    public void scanExp() {
        setupEngine();
        goToPosition(stPos);
        startMeasure();
        scan.start();
        startTime = System.currentTimeMillis();
        goToPosition(endPos);
        scanning = false;
        try {
            Thread.sleep(500);
        } catch (InterruptedException ex) {
            Logger.getLogger(EndoscopExp.class.getName()).log(Level.SEVERE, null, ex);
        }
        endMeasure();
        try {
            scan.join();
        } catch (InterruptedException ex) {
            Logger.getLogger(EndoscopExp.class.getName()).log(Level.SEVERE, null, ex);
        }
    }

    public void handManipulatingExp() {
        eChannel.getGear().getActuator().setTorqueEnable(false);
        try {
            Thread.sleep(500);
        } catch (InterruptedException ex) {
            Logger.getLogger(EndoscopExp.class.getName()).log(Level.SEVERE, null, ex);
        }
        waitPos(true);
        startMeasure();
        scan.start();
        startTime = System.currentTimeMillis();
        waitPos(false);
        scanning = false;
        endMeasure();
        try {
            scan.join();
        } catch (InterruptedException ex) {
            Logger.getLogger(EndoscopExp.class.getName()).log(Level.SEVERE, null, ex);
        }
    }

    public double getStPos() {
        return stPos;
    }

    public double getEndPos() {
        return endPos;
    }

    public double getStPos_adc() {
        return stPos_adc;
    }

    public double getStPos_eng() {
        return stPos_eng;
    }

    public double getEndPos_adc() {
        return endPos_adc;
    }

    public double getEndPos_eng() {
        return endPos_eng;
    }

    public int getSrcStPos_adc() {
        return srcStPos_adc;
    }

    public int getSrcStPos_eng() {
        return srcStPos_eng;
    }

    public int getSrcEndPos_adc() {
        return srcEndPos_adc;
    }

    public int getSrcEndPos_eng() {
        return srcEndPos_eng;
    }

    public double getStTor_eng() {
        return stTor_eng;
    }

    public double getEndTor_eng() {
        return endTor_eng;
    }

    public int getSrcStTor_eng() {
        return srcStTor_eng;
    }

    public int getSrcEndTor_eng() {
        return srcEndTor_eng;
    }

    public EndoscopeChannel getChannel() {
        return eChannel;
    }

    public double getStTor_adc() {
        return stTor_adc;
    }

    public double getEndTor_adc() {
        return endTor_adc;
    }

    public int getSrcStTor_adc() {
        return srcStTor_adc;
    }

    public int getSrcEndTor_adc() {
        return srcEndTor_adc;
    }

    public double getCoefOffsetCalibTorqueAdc() {
        return coefOffsetCalibTorqueAdc;
    }

    public double getCoefPropCalibTorqueAdc() {
        return coefPropCalibTorqueAdc;
    }

    public void setTorqueLimit(double limit) {
        eChannel.getGear().setTorque(limit);
    }

    public int getSize() {
        return size;
    }

    public double[] getPosEng() {
        double res[] = new double[size];
        System.arraycopy(posEng, 0, res, 0, size);
        return res;
    }

    public double[] getTorEng() {
        double res[] = new double[size];
        System.arraycopy(torEng, 0, res, 0, size);
        return res;
    }

    public double[] getPosAdc() {
        double res[] = new double[size];
        System.arraycopy(posAdc, 0, res, 0, size);
        return res;
    }

    public double[] getTorAdc() {
        double res[] = new double[size];
        System.arraycopy(torAdc, 0, res, 0, size);
        return res;
    }

    public double[] getTime() {
        double res[] = new double[size];
        System.arraycopy(time, 0, res, 0, size);
        return res;
    }

    private void waitPos(boolean isStart) {
        int steps = 0;
        int curTor = -1;
        int oldTor = -1;
        while (curTor == -1) {
            curTor = eChannel.getGear().getActuator().getCurTorque();
        }
        oldTor = curTor;
        while (oldTor == curTor) {
            curTor = eChannel.getGear().getActuator().getCurTorque();
            if (curTor == -1) {
                curTor = oldTor;
            }
            try {
                Thread.sleep(5);
            } catch (InterruptedException ex) {
                Logger.getLogger(EndoscopExp.class.getName()).
                        log(Level.SEVERE, null, ex);
            }
        }
        while (!timeOut) {
            curTor = eChannel.getGear().getActuator().getCurTorque();
            while (oldTor != curTor && curTor != -1) {
                oldTor = curTor;
                curTor = eChannel.getGear().getActuator().getCurTorque();
                steps = 0;
            }
            steps++;
            try {
                Thread.sleep(10);
            } catch (InterruptedException ex) {
                Logger.getLogger(EndoscopExp.class.getName()).
                        log(Level.SEVERE, null, ex);
            }
            if (steps > 20) {
                timeOut = true;
            }
        }
        timeOut = false;
        if (isStart) {
            Tools.playNote(Note.p87_7_Re_Diese, Note.i98_FX_3_crystal, 127, 1000);
        } else {
            Tools.playNote(Note.p99_8_Re_Diese, Note.i98_FX_3_crystal, 127, 1000);
        }
    }

    /**
     * source start pos adc , start pos adc source start pos eng , start pos eng
     * source start tor eng, start tor eng source start tor adc, start tor adc
     */
    private void startMeasure() {
        srcStPos_eng = eChannel.getGear().getActuator().getCurPos();
        stPos_eng = eChannel.getGear().getPosition(srcStPos_eng);
        srcStTor_eng = eChannel.getGear().getActuator().getCurTorque();
        stTor_eng = eChannel.getGear().getTorque(srcStTor_eng);

        /*srcStPos_adc = eChannel.getAdcSourceData();
         stPos_adc = eChannel.getAdcData(srcStPos_adc);
         srcStTor_adc = eChannel.getTorqueSourceData();
         stTor_adc = eChannel.getTorqueData(srcStTor_adc);*/
        /*Random rand = new Random();
         stPos_adc = stPos_eng * (rand.nextDouble() / 10 + 0.95);
         stTor_adc = stTor_eng * (rand.nextDouble() / 10 + 0.95);
         */

    }

    /**
     * Source end pos adc , end pos adc Source end pos eng , end pos eng Source
     * end torque eng, end torque eng Source end torque adc , end torque adc
     * This method also calculates coefficients of proportionality and offsets
     * for ADC and engine respectively.
     */
    private void endMeasure() {
        srcEndPos_eng = eChannel.getGear().getActuator().getCurPos();
        endPos_eng = eChannel.getGear().getPosition(srcEndPos_eng);
        srcEndTor_eng = eChannel.getGear().getActuator().getCurTorque();
        endTor_eng = eChannel.getGear().getTorque(srcEndTor_eng);
        coefPropCalibPosAdc = (endPos_eng - stPos_eng)
                / (double) (srcEndPos_adc - srcStPos_adc);
        coefOffsetCalibPosAdc =
                endPos_eng - srcEndPos_adc * coefPropCalibPosAdc;
        coefPropCalibTorqueAdc = (endTor_eng - stTor_eng)
                / (double) (srcEndTor_adc - srcStTor_adc);
        coefOffsetCalibTorqueAdc =
                endTor_eng - srcEndTor_adc * coefPropCalibTorqueAdc;

        /*srcEndPos_adc = eChannel.getAdcSourceData();
         endPos_adc = eChannel.getAdcData(srcEndPos_adc);
         srcEndTor_adc = eChannel.getTorqueSourceData();
         endTor_adc = eChannel.getTorqueData(srcEndTor_adc);*/
        if (airship) {
                Random rand = new Random();
            if (size > 1) {

                endPos_adc = posAdc[size - 2];
                endTor_eng = torEng[size-2];
                endTor_adc = torAdc[size - 2] ;

                stPos_adc = posAdc[0];
                stTor_eng = torEng[0];
                stTor_adc = torAdc[0] ;
            } else {
                stPos_adc = stPos_eng * (rand.nextDouble() / 10 + 0.95);
                stTor_adc = stTor_eng * (rand.nextDouble() / 10 + 0.95);
                endPos_adc = endPos_eng * (rand.nextDouble() / 10 + 0.95);
                endTor_adc = endTor_eng * (rand.nextDouble() / 10 + 0.95);
            }
        }
        System.out.println("Airship status -> " + airship);
        System.out.println("Start torque (eng): " + stTor_eng + " |"
                + " Start torque(adc)" + stTor_adc +  " | End torque(eng)"
                + endTor_eng + " | End torque(adc)" + endTor_adc) ;
    }

    /**
     * Waiting until engine get to specified position
     *
     * @param pos
     */
    private void goToPosition(double pos) {
        eChannel.getGear().setPosition(pos);
        while (eChannel.getGear().getActuator().isMoving()) {
            try {
                Thread.sleep(200);
            } catch (InterruptedException ex) {
                Logger.getLogger(EndoscopExp.class.getName()).log(Level.SEVERE, null, ex);
            }
        }
        try {
            Thread.sleep(500);
        } catch (InterruptedException ex) {
            Logger.getLogger(EndoscopExp.class.getName()).log(Level.SEVERE, null, ex);
        }
    }

    private void setupEngine() {
        eChannel.getGear().getActuator().setPID(80, 0, 0);
        eChannel.getGear().getActuator().setSpeed(25, true);
    }

    public double getAbsError() {
        return Math.abs((endPos_eng - stPos_eng) - (endPos_adc - stPos_adc));
    }

    public double getAbsErrorTorq() {
        return Math.abs((endTor_eng - stTor_eng) - (endTor_adc - stTor_adc));
    }

    public double getRelError() {
        return Math.abs(((endPos_eng - stPos_eng) - (endPos_adc - stPos_adc)) / (endPos_adc - stPos_adc));
    }

    public double getRelErrorTorq() {
        return Math.abs(((endTor_eng - stTor_eng) - (endTor_adc - stTor_adc))
                / (endTor_adc - stTor_adc));
    }

    public double getSigma() {
        if (size > 0) {
            double sum = 0;
            for (int i = 0; i < size; i++) {
                sum += (posEng[i] - posAdc[i]) * (posEng[i] - posAdc[i]) / size;
            }
            return Math.sqrt(sum);
        } else {
            return -1;
        }
    }

    public double getSigmaTor() {
        if (size > 0) {
            double sum = 0;
            for (int i = 0; i < size; i++) {
                sum += (torEng[i] - torAdc[i]) * (torEng[i] - torAdc[i]) / size;
            }
            return Math.sqrt(sum);

        } else {
            return -1;
        }
    }

    public double getMaxAbsError() {
        if (size > 0) {
            double max = Math.abs(posEng[0] - posAdc[0]);
            for (int i = 1; i < size; i++) {
                if (max < Math.abs(posEng[i] - posAdc[i])) {
                    max = Math.abs(posEng[i] - posAdc[i]);
                }
            }
            return max;
        } else {
            return -1;
        }
    }
    
    public double getMaxAbsErrorTor(){
        if (size > 0)  {
            double max = Math.abs(torEng[0] - torAdc[0]);
            for (int i = 1; i < size; i++) {
                if (max < Math.abs(torEng[i] - torAdc[i])) {
                    max = Math.abs(torEng[i] - posAdc[i]);
                }
            }
            return max ;
        } else {
            return -1 ;
        }
    } 

    public void setAirship(boolean airship) {
        this.airship = airship;
    }

    public double getCurEngVal() {
        return posEng[size];
    }

    public double getCurAdcVal() {
        return posAdc[size];
    }

    public void addListenersList(ArrayList<GraphListener> list) {
        scanList.addAll(list);
    }

    public void addScanListener(GraphListener sl) {
        scanList.add(sl);
    }

    public void removeScanListener(int indx) {
        scanList.remove(indx);
    }

    public void clearScanList() {
        scanList.clear();
    }

    public void translateToPlots(double eng, double adc) {
        for (int i = 0; i < scanList.size(); i++) {
            scanList.get(i).addValue(eng, adc);
        }
    }
     public void clearPlots() {
        for (int i = 0; i < scanList.size(); i++) {
            scanList.get(i).clear();
        }
    }
   
    
    private void detectTorque() {
        Random r = new Random();
        try {
            Thread.sleep((int)(8+(r.nextDouble()*8)));
        } catch (InterruptedException ex) {
            Logger.getLogger(EndoscopExp.class.getName()).log(Level.SEVERE, null, ex);
        }
        
    }
    
    public double getMeasuredTime(){
        return measureTime ;
    }
}